#include "camControl/robot.hpp"

bool Robot::setRobotInfo(INFO_ROBOT info) {

    if (this->ok) {

        this->info = info;

        return true;
    } else
        return false;
}

INFO_ROBOT Robot::getRobotInfo() {

    INFO_ROBOT info;

    info = this->info;

    return info;
}

//void Robot::setupKalmanFilter(int DM, int MM, int CM,
//        double* F, double* B, double* H, double* Q, double* R,
//        double* error_covariance, double* state) {
//
//    this->ok = true;
//    
//    this->kalman = new Kalman<float>(DM, MM, CM,
//            F, B, H, Q, R, error_covariance, state);
//}

void Robot::setupKalmanFilter() {

    this->ok = true;
    
    this->kalman = new Kalman<float>();
}
